/*
 * @Description: 坐标、速度的定义与实现
 * @Author: Dryad
 * @Date: 2019-06-17 23:01:31
 */
#pragma once
#include <arm_math.h>

/* 坐标 */
class Coordinate_Class
{
public:
    union {
        struct
        {
            float x_coor;     /* x坐标(mm) */
            float y_coor;     /* y坐标(mm) */
            float angle_coor; /* 角度坐标(°) */
        };
        float coor[3];
    };

    void Clear(void);                                                   /* 坐标清零 */
    float Trans_Rad(void) const;                                        /* 转换为弧度 */
    static float Trans_Rad(float angle) { return angle / 180.0f * PI; } /* 转换为弧度 */
    void Angle_Trans(const float base_angle);                           /* 将角度转换至base_angle的(-180,+180]周期内 */
    static float Angle_Trans(float angle, const float base);            /* 将角度angle转换至base_angle的(-180,+180]周期内 */

    Coordinate_Class &operator+=(const Coordinate_Class &addend);     /* 求解在当前坐标系中，坐标为addend的绝对坐标 */
    Coordinate_Class &operator-=(const Coordinate_Class &subtrahend); /* 求解当前绝对坐标在subtrahend坐标系中的坐标 */

    /* 相对坐标转化为绝对坐标 */
    static Coordinate_Class &Relative_To_Absolute(Coordinate_Class &Absolute_Coor,       /* 世界坐标系下的坐标 */
                                                  const Coordinate_Class &Relative_Coor, /* Base_Coor坐标系下的坐标 */
                                                  const Coordinate_Class &Base_Coor);    /* 世界坐标系下的基坐标 */
    /* 绝对坐标转化为相对坐标 */
    static Coordinate_Class &Absolute_To_Relative(const Coordinate_Class &Absolute_Coor, /* 世界坐标系下的坐标 */
                                                  Coordinate_Class &Relative_Coor,       /* Base_Coor坐标系下的坐标 */
                                                  const Coordinate_Class &Base_Coor);    /* 世界坐标系下的基坐标 */
};

/* 求解在summand坐标系中相对坐标为addend的坐标系在全局坐标系中的坐标 */
Coordinate_Class operator+(const Coordinate_Class &summand, const Coordinate_Class &addend);
/* 求解绝对坐标minuend在subtrahend坐标系中的坐标 */
Coordinate_Class operator-(const Coordinate_Class &minuend, const Coordinate_Class &subtrahend);
/* 坐标系放大 */
Coordinate_Class operator*(const Coordinate_Class &source, const float factor);

/* 速度 */
class Velocity_Class
{
public:
    union {
        struct
        {
            float velocity_x;     /* x方向速度 mm/s */
            float velocity_y;     /* y方向速度 mm/s */
            float velocity_angle; /* 角速度 °/s */
        };
        float coor[3];
    };

    void Clear(void);                                                                      /* 速度清零 */
    float Trans_Rad(void) const;                                                           /* 速度转换为rad/s */
    static float Trans_Rad(float velocity_angle) { return velocity_angle / 180.0f * PI; }; /* 速度转换为rad/s */

    Velocity_Class &operator*=(const float factor);
    Velocity_Class &operator/=(const float divisor);

    static Velocity_Class &Relative_To_Absolute(Velocity_Class &Absolute_Velocity, const Velocity_Class &Relative_Velocity, const Coordinate_Class &Base_Coor); /* 从相对速度转换为绝对速度 */
    static Velocity_Class &Absolute_To_Relative(const Velocity_Class &Absolute_Velocity, Velocity_Class &Relative_Velocity, const Coordinate_Class &Base_Coor); /* 从绝对速度转换为相对速度 */
};
